Occupancy Grid Mapping

Star Discuss Download
In this mapping method, the 2-D world to be mapped is divided into evenly spaced grids, each grid has one of the three possible states: free, occupied and unknown. The robot wandering around in the world and take measurements (e.g. lidar scan) along the way, to obtain the pair , these pairs are used to generate the map.
The map is represented with a matrix, with each element corresponding to the state of a grid in the world. At each timestamp, the belief of state of the grid is updated according to binary (free vs occupied) Bayes filter using the old belief and the measurement (inverse measurement model i.e. ) (grid with unknown state is left untouched).
occupancyGridMapping.gif
% Load the map, prerecorded poses and measurements.
%
% The world is 27 meters in width by 26 meters, and the robot drives along
% a pre-determined path in the world, and taking lidar scans along the way.
%
% Theese data are recorded using the Matlab example
% "MappingWithKnownPosesDiffDriveExample"
load('sensorData.mat');
% These are the robot's pose at each timestamp (x, y, theta),
% and 258 ranges and the corresponding angles (relative to the robot)
% at which the range is measured
robotPoses = allPoses;
sensorRanges = allScannedRanges';
sensorAngles = allScannedAngles';
% Use only part of the data
robotPoses = robotPoses(1:858, :);
sensorRanges = sensorRanges(1:858, :);
sensorAngles = sensorAngles(1:858, :);
% Small data for dev
% robotPoses = robotPoses(150:200, :);
% sensorRanges = sensorRanges(150:200, :);
% sensorAngles = sensorAngles(150:200, :);
% Show the reference map and the path on the left
refMap = binaryOccupancyMap(simpleMap,1);
subplot(1, 2, 1);
hold on
show(refMap);
path = [4 6; 6.5 12.5; 4 22; 12 14; 22 22; 16 12; 20 10; 14 6; 22 3];
plot(path(:,1),path(:,2), 'o-');
mapWidth = 27;
mapHeight = 26;
gridSize = 0.1;
% The belief represents the map,
% higher value means more likely to be occupied
cols = mapWidth / gridSize;
rows = mapHeight / gridSize;
belief = zeros([rows, cols]);
initialBelief = zeros([rows, cols]);
% Live plot for the mapping process
subplot(1, 2, 2);
% Loop over the (pose, measurement) pairs
tic;
[numSensorDatas, ~] = size(sensorRanges);
pbar = waitbar(0, "Mapping", 'Name', "Mapping...");
for i = 1:numSensorDatas
hold off;
progress = i / numSensorDatas;
waitbar(progress, pbar, sprintf("%.2f%%", progress * 100));
% Process one measurement
range = sensorRanges(i, :);
angle = sensorAngles(i, :);
pose = robotPoses(i, :);
% Loop over the the grids
% TODO: this is painfully slow
parfor r = 1:rows
for c = 1:cols
% Get the coordinate of the grid
x = (c - 1) * gridSize + gridSize / 2.0;
y = (rows - r) * gridSize + gridSize / 2.0;
% Calculate the occupancy state of the grid
newBelief = inverseSensorModel(x, y, range, angle, pose);
if isfinite(newBelief)
belief(r, c) = belief(r, c) + newBelief - initialBelief(r, c);
end
end
end
% Plot the current belief along with the location of the robot
p = 1 ./ (1 + exp(belief));
imshow(p);
hold on;
title("Constructed Map");
xlim([0, cols]);
ylim([0, rows]);
plot(path(:, 1) / gridSize, rows-path(:, 2) / gridSize, 'o-');
px = pose(1, 1) / gridSize;
py = rows - pose(1, 2) / gridSize;
plot(px, py, 'r+');
% Save to a gif
frame = getframe(gcf);
img = frame2im(frame);
[img,cmap] = rgb2ind(img, 256);
if i == 1
imwrite(img,cmap, 'occupancyGridMapping.gif', 'gif', 'LoopCount', Inf, 'DelayTime', 1/60);
else
imwrite(img,cmap, 'occupancyGridMapping.gif', 'gif', 'WriteMode', 'append', 'DelayTime', 1/60);
end
end
toc;
Elapsed time is 720.287976 seconds.
close(pbar);
function negLogOdds = inverseSensorModel(xi, yi, range, angle, pose)
% range of the lidar sensor
zMax = 10;
zMin = 0;
% special value to indicate unknown state
lUnknown = inf;
lFree = -0.4;
lOccupied = 0.4;
% size of the grid
alpha = 0.1;
% remove invalid scans before doing anything interesting
good = find(isfinite(range) & range >= zMin & range <= zMax);
ranges = range(:, good);
angles = angle(:, good);
x = pose(1, 1);
y = pose(1, 2);
theta = pose(1, 3);
% distance between the robot and grid
r = norm([xi-x, yi-y]);
% angle of the grid (xi, yi) relative to the robot
phi = atan2((yi-y), (xi-x)) - theta;
% find the closest sensor to the grid
[minAngle, k] = min(abs(angles - phi));
zK = ranges(1, k);
% determine the state
if minAngle >= pi /2 ...
|| (r > alpha / 2 && tan(minAngle) > (alpha / 2) / (r - alpha / 2))
negLogOdds = lUnknown;
elseif abs(r - zK) <= alpha / 2
negLogOdds = lOccupied;
elseif r < zK - alpha / 2
negLogOdds = lFree;
else
negLogOdds = lUnknown;
end
end